#include "AutonomousHandler.h"

namespace Team675
{

AutonomousHandler::AutonomousHandler(AutonomousMode _mode, Team675::RobotDrive* _drive, Team675::RobotPayload* _payload)
{
	m_autonomousMode = _mode;
	m_robotDrive = _drive;
	m_robotPayload = _payload;
	m_driveType = NULL;
	
	m_autonomousStarted = false;
	m_autonomousTimer.Stop();
	m_autonomousTimer.Reset();
}

AutonomousHandler::~AutonomousHandler()
{
	delete m_driveType;
}

void AutonomousHandler::Update()
{
	if(!m_autonomousStarted)
	{
		m_autonomousTimer.Start();
		m_autonomousStarted = true;
	}
	
//	m_autonomousMode = *((AutonomousMode*) m_modeChooser->GetSelected());
	
	switch(m_autonomousMode)
	{
	case AUTONOMOUS_KINECT:
		// Drive
		if(m_driveType == NULL)
		{
			m_driveType = new KinectDrive();
			m_robotDrive->SetDrive(m_driveType);
		}
		
		// Payload
		m_robotPayload->SetControlMode(RobotPayload::CONTROL_KINECT);
	break;
	
	case AUTONOMOUS_STILL:
		// Drive
		if(m_driveType == NULL)
		{
			m_driveType = new AutonomousDrive();
			((AutonomousDrive*) m_driveType)->Set(0.0, 0.0, true);
			m_robotDrive->SetDrive(m_driveType);
		}
		
		// Payload
		m_robotPayload->SetControlMode(RobotPayload::CONTROL_AUTONOMOUS);
		m_robotPayload->SetShootingMode(RobotPayload::SHOOTING_DISABLED);
		if((m_autonomousTimer.Get() > AUTONOMOUS_SHOOT_DELAY && m_autonomousTimer.Get() < AUTONOMOUS_SHOOT_DELAY + BELT_SHOOT_TIME) || m_autonomousTimer.HasPeriodPassed(AUTONOMOUS_SHOOT_DELAY + BELT_SHOOT_TIME + 1.0))
		{
			m_robotPayload->SetShootingMode(RobotPayload::SHOOTING_AUTOMATIC);
		}
	break;
	
	case AUTONOMOUS_FORWARD:
		// Drive
		if(m_driveType == NULL)
		{
			m_driveType = new AutonomousDrive();
			m_robotDrive->SetDrive(m_driveType);
		}
		
		// Payload
		m_robotPayload->SetControlMode(RobotPayload::CONTROL_AUTONOMOUS);
	break;
	
	case AUTONOMOUS_CENTER:
		// Drive
		if(m_driveType == NULL)
		{
			m_driveType = new AutonomousDrive();
			m_robotDrive->SetDrive(m_driveType);
		}
		
		// Payload
		m_robotPayload->SetControlMode(RobotPayload::CONTROL_AUTONOMOUS);
	break;
	
	default:
	break;
	}
	
	m_robotDrive->Update();
	m_robotPayload->Update();
}

void AutonomousHandler::InitChooser()
{
	m_modeChooser = new SendableChooser;
	m_modesList = new AutonomousMode[AUTONOMOUS_MODES];
	
	m_modesList[0] = AUTONOMOUS_KINECT;
	m_modesList[1] = AUTONOMOUS_STILL;
	m_modesList[2] = AUTONOMOUS_FORWARD;
	m_modesList[3] = AUTONOMOUS_CENTER;
	m_modesList[4] = AUTONOMOUS_DISABLED;
	
	m_modeChooser->AddDefault("Disabled", &m_modesList[4]);
	m_modeChooser->AddObject("Kinect Control", &m_modesList[0]);
	m_modeChooser->AddObject("Just Shoot", &m_modesList[1]);
	m_modeChooser->AddObject("Forward then Shoot", &m_modesList[2]);
	m_modeChooser->AddObject("Center Ramp", &m_modesList[3]);
	
	SmartDashboard::GetInstance()->PutData("Autonomous Chooser", m_modeChooser);
}

}
